/*
建图模块：
接收里程计模块发送的里程计信息
接收Odometry节点发送的
执行5hz的建图频率
*/
#include "common.h"
#include <cmath>
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <Eigen/Dense>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_broadcaster.h>

using namespace std;
using namespace Eigen;

//扫描周期
const float scanPeriod = 0.1;

//控制接收到的点云数据，每隔几帧处理一次
const int stackFrameNum = 1;
//控制处理得到的点云map，每隔几次publich给rviz显示
const int mapFrameNum = 5;

//时间戳
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;

//接收标志
bool newLaserCloudCornerLast = false;
bool newLaserCloudSurfLast = false;
bool newLaserCloudFullRes = false;
bool newLaserOdometry = false;

//分块 地图在数组方块中的坐标
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
//点云方块集合最大数量
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851

//lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];

//存储最新接收到的边缘点 局部坐标系
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//存储最新接收到的平面点 局部坐标系
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//存储最新接收到的点云全部点 局部坐标系
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());

//存放当前接收到的边缘点 作为下采样的数据源 世界坐标系点
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack2(new pcl::PointCloud<PointType>());
//存放当前接收到的平面点 作为下采样的数据源 世界坐标系点
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack2(new pcl::PointCloud<PointType>());
//存放当前接收到的边缘点下采样后的边缘点
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
//存放当前接收到的平面点下采样后的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());

//原始点云坐标
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之后的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之前的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround2(new pcl::PointCloud<PointType>());
//存储边缘点的cube 立方体指针数组
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
//存放平面点的cube 立方体指针数组
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
//中间变量：存放下采样后的边缘点cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray2[laserCloudNum];
//中间变量：存放下采样后的平面点cube
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray2[laserCloudNum];

//map中提取的匹配使用的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
//map中提取的匹配使用的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());

//kd-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());

/*************高频转换量**************/
//odometry计算得到的到世界坐标系下的转移矩阵 接收到的odom数据
float transformSum[6] = {0};
//转移增量，只使用了后三个平移增量
float transformIncre[6] = {0};

/*************低频转换量*************/
//以起始位置为原点的世界坐标系下的转换矩阵（猜测与调整的对象）
float transformTobeMapped[6] = {0};
//存放mapping之前的Odometry计算的世界坐标系的转换矩阵（注：低频量，不一定与transformSum一样）
float transformBefMapped[6] = {0};
//存放mapping之后的经过mapping微调之后的转换矩阵
float transformAftMapped[6] = {0};

//根据匀速运动模型 根据上次微调的结果和Odometry上次与这次的计算结果
//猜测新的转换矩阵transformTobeMapped
void transformAssociatiateToMap(){

    float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
             - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
    float y1 = transformBefMapped[4] - transformSum[4];
    float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
            + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);

    float x2 = x1;
    float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
    float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;

    //平移增量
    transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
    transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
    transformIncre[5] = z2;

    float sbcx = sin(transformSum[0]);
    float cbcx = cos(transformSum[0]);
    float sbcy = sin(transformSum[1]);
    float cbcy = cos(transformSum[1]);
    float sbcz = sin(transformSum[2]);
    float cbcz = cos(transformSum[2]);

    float sblx = sin(transformBefMapped[0]);
    float cblx = cos(transformBefMapped[0]);
    float sbly = sin(transformBefMapped[1]);
    float cbly = cos(transformBefMapped[1]);
    float sblz = sin(transformBefMapped[2]);
    float cblz = cos(transformBefMapped[2]);

    float salx = sin(transformAftMapped[0]);
    float calx = cos(transformAftMapped[0]);
    float saly = sin(transformAftMapped[1]);
    float caly = cos(transformAftMapped[1]);
    float salz = sin(transformAftMapped[2]);
    float calz = cos(transformAftMapped[2]);

    float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
                - cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
                - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
                - cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) 
                - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
    transformTobeMapped[0] = -asin(srx);

    float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
                - cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
                - cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
                + (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
                + cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
                + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
    float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
                - cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
                + cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
                + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
                - cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
                + (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
    transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), 
                                    crycrx / cos(transformTobeMapped[0]));
    
    float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
                - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
                - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
                - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
                + cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
    float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
                - calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
                - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
                - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
                + cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
    transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), 
                                    crzcrx / cos(transformTobeMapped[0]));

    x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
    y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
    z1 = transformIncre[5];

    x2 = x1;
    y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
    z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

    transformTobeMapped[3] = transformAftMapped[3] 
                            - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
    transformTobeMapped[4] = transformAftMapped[4] - y2;
    transformTobeMapped[5] = transformAftMapped[5] 
                            - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
    
}

//记录odometry发送的变换矩阵与更新之后的变换矩阵
void transformUpdate(){
    //记录优化之前与优化之后的变换矩阵
    for (int i = 0; i < 6; i++){
        transformBefMapped[i] = transformSum[i];
        transformAftMapped[i] = transformTobeMapped[i];
    }
}

//将局部坐标系的点变换到世界坐标系
void pointAssociateToMap(PointType const* const pi, PointType * const po){
    //绕z轴旋转（transformTobeMapped[2]）
    float x1 = cos(transformTobeMapped[2]) * pi->x
            - sin(transformTobeMapped[2]) * pi->y;
    float y1 = sin(transformTobeMapped[2]) * pi->x
            + cos(transformTobeMapped[2]) * pi->y;
    float z1 = pi->z;

    //绕x轴旋转（transformTobeMapped[0]）
    float x2 = x1;
    float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
    float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

    //绕y轴旋转（transformTobeMapped[1]），再平移
    po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2
            + transformTobeMapped[3];
    po->y = y2 + transformTobeMapped[4];
    po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2
            + transformTobeMapped[5];
    po->intensity = pi->intensity;

}
//点转移到局部坐标系下
void pointAssociateTobeMapped(PointType const * const pi, PointType * const po){

    //平移后绕y轴旋转（-transformTobeMapped[1]）
    float x1 = cos(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) 
            - sin(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]);
    float y1 = pi->y - transformTobeMapped[4];
    float z1 = sin(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3]) 
            + cos(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]);

    //绕x轴旋转（-transformTobeMapped[0]）
    float x2 = x1;
    float y2 = cos(transformTobeMapped[0]) * y1 + sin(transformTobeMapped[0]) * z1;
    float z2 = -sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

    //绕z轴旋转（-transformTobeMapped[2]）
    po->x = cos(transformTobeMapped[2]) * x2
            + sin(transformTobeMapped[2]) * y2;
    po->y = -sin(transformTobeMapped[2]) * x2
            + cos(transformTobeMapped[2]) * y2;
    po->z = z2;
    po->intensity = pi->intensity;
}


//接收laser_cloud_corner_last
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr&  laserCloudCornerLast2){
    //时间戳
    timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec();
    //清空容器
    laserCloudCornerLast->clear();
    //消息类型转化为点云类型
    pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);
    newLaserCloudCornerLast = true;
} 
//接收laser_cloud_surf_last
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2){
    timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec();
    laserCloudSurfLast->clear();
    pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);
    newLaserCloudSurfLast = true;
}
//接收点云全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2){
    timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();
    laserCloudFullRes->clear();
    pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
    newLaserCloudFullRes = true;
}
//接收laser_odom_to_init 这部分我进行了修改
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){
    timeLaserOdometry = laserOdometry->header.stamp.toSec();
    //四元数转化为欧拉角 欧拉角顺序为ZXY
    Eigen::Quaterniond q;
    q.coeffs().x() = laserOdometry->pose.pose.orientation.x;
    q.coeffs().y() = laserOdometry->pose.pose.orientation.y;
    q.coeffs().z() = laserOdometry->pose.pose.orientation.z;
    q.coeffs().w() = laserOdometry->pose.pose.orientation.w;

    Eigen::Vector3d eulerAngle = q.matrix().eulerAngles(2,0,1);
    transformSum[0] = eulerAngle[1];
    transformSum[1] = eulerAngle[2];
    transformSum[2] = eulerAngle[0];

    transformSum[3] = laserOdometry->pose.pose.position.x;
    transformSum[4] = laserOdometry->pose.pose.position.y;
    transformSum[5] = laserOdometry->pose.pose.position.z;
    
    newLaserOdometry  = true;

} 

int main(int argc, char** argv){
    //初始化节点
    ros::init(argc, argv, "laserMapping");
    ros::NodeHandle nh;

    ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>
                                             ("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);

    ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>
                                            ("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);

    ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
                                        ("/laser_odom_to_init", 5, laserOdometryHandler);

    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> 
                                            ("/velodyne_cloud_3", 2, laserCloudFullResHandler);

    ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> 
                                            ("/laser_cloud_surround", 1);

    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> 
                                            ("/velodyne_cloud_registered", 2);

    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
    nav_msgs::Odometry odomAftMapped;
    odomAftMapped.header.frame_id = "/camera_init";
    odomAftMapped.child_frame_id = "/aft_mapped";

    tf::TransformBroadcaster tfBroadcaster;
    tf::StampedTransform aftMappedTrans;
    aftMappedTrans.frame_id_ = "/camera_init";
    aftMappedTrans.child_frame_id_ = "/aft_mapped";

    //最近点的index 
    std::vector<int> pointSearchInd;
    //最近点的距离 float
    std::vector<float> pointSearchSqDis;

    //原始点 被选点 系数；
    PointType pointOri, pointSel, coeff;

    cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));
    cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));
    cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));
    //边沿点的协方差矩阵 特征值和特征向量
    cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
    cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
    cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));

    //是否退化
    bool isDegenerate = false;
    cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
    

    //创建VoxelGrid滤波器 边缘点
    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    //设置体素大小
    downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
    //创建VoxelGrid滤波器 表面点
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
    //创建VoxelGrid滤波器 地图点
    pcl::VoxelGrid<PointType> downSizeFilterMap;
    downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);

    //指针初始化 指针数组中点云指针初始化
    for (int i = 0; i < laserCloudNum; i++){
        laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
        laserCloudCornerArray2[i].reset(new pcl::PointCloud<PointType>());
        laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
        laserCloudSurfArray2[i].reset(new pcl::PointCloud<PointType>());
    }
    //将接收到的帧数初始化为0
    int frameCount = stackFrameNum - 1;     //0
    //将接收到的地图帧数初始化为4 因为第一帧算一个地图 之后每五帧发送一次
    int mapFrameCount = mapFrameCount - 1;  //4

    ros::Rate rate(100);
    bool status = ros::ok();
    while (status){
        //开始执行
        ros::spinOnce();
        if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
            fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
            fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
            fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {
                newLaserCloudCornerLast = false;
                newLaserCloudSurfLast = false;
                newLaserCloudFullRes = false;
                newLaserOdometry = false;
                
                //接收到新一帧
                frameCount++;
                //接收到新一帧 猜测一个变换矩阵 在转换到世界坐标系下
                if (frameCount >= stackFrameNum){
                    //推测一个变换矩阵初值
                    transformAssociatiateToMap();
                    //将最新接收到的边缘点和平面点转换到世界坐标系下
                    int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
                    for (int i = 0; i < laserCloudCornerLastNum; i++){
                        pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
                        laserCloudCornerStack2->push_back(pointSel);
                    }

                    int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
                    for (int i = 0; i < laserCloudSurfLastNum; i++){
                        pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
                        laserCloudSurfStack2->push_back(pointSel);
                    }
                }

                //优化处理
                if (frameCount >= stackFrameNum){
                    frameCount = 0;
                    //y轴上的一个点
                    PointType pointOnYAxis;
                    pointOnYAxis.x = 0.0;
                    pointOnYAxis.y = 10.0;
                    pointOnYAxis.z = 0.0;
                    //获取y方向上10米高位置的点在世界坐标系下的坐标
                    pointAssociateToMap(&pointOnYAxis, & pointOnYAxis);

                    //当前坐标系所在立方体中的位置
                    int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
                    int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
                    int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;

                    //由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一
                    if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
                    if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
                    if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;

                    //为了使立方体数组利用最大化
                    //调整之后取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18
                    //如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位
                    while (centerCubeI < 3){
                        for (int j = 0; j < laserCloudHeight; j ++){
                            for (int k = 0; k < laserCloudDepth; k++){
                                //向后移位 现将最后一个保存下来 最后赋值给第一个
                                int i = laserCloudWidth - 1;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer = 
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer = 
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //循环移位
                                for (; i >= 1; i--){
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                    laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight *k];
                                    
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                    laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudCenHeight *k];

                                }
                                //将最后一个点赋值给第一个点
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudCenHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
    
                            }
                        }
                        centerCubeI++;
                        laserCloudCenWidth++;
                    }
                    //如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
                    while (centerCubeI >= laserCloudWidth - 3) {//18
                        for (int j = 0; j < laserCloudHeight; j++) {
                            for (int k = 0; k < laserCloudDepth; k++) {
                                int i = 0;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //I维度上依次前移
                                for (; i < laserCloudWidth - 1; i++) {
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                }
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
                            }
                        }

                        centerCubeI--;
                        laserCloudCenWidth--;
                    }

                    while (centerCubeJ < 3) {
                        for (int i = 0; i < laserCloudWidth; i++) {
                            for (int k = 0; k < laserCloudDepth; k++) {
                                int j = laserCloudHeight - 1;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //J维度上，依次后移
                                for (; j >= 1; j--) {
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k];
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];
                                }
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
                            }
                        }
                
                        centerCubeJ++;
                        laserCloudCenHeight++;
                    } 

                    while (centerCubeJ >= laserCloudHeight - 3) {
                        for (int i = 0; i < laserCloudWidth; i++) {
                            for (int k = 0; k < laserCloudDepth; k++) {
                                int j = 0;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //J维度上一次前移
                                for (; j < laserCloudHeight - 1; j++) {
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k];
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];
                                }
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
                            }
                        }

                        centerCubeJ--;
                        laserCloudCenHeight--;
                    }

                    while (centerCubeK < 3) {
                        for (int i = 0; i < laserCloudWidth; i++) {
                            for (int j = 0; j < laserCloudHeight; j++) {
                                int k = laserCloudDepth - 1;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //K维度上依次后移
                                for (; k >= 1; k--) {
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)];
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];
                                }
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
                            }
                        }

                        centerCubeK++;
                        laserCloudCenDepth++;
                    }
                
                    while (centerCubeK >= laserCloudDepth - 3) {
                        for (int i = 0; i < laserCloudWidth; i++) {
                            for (int j = 0; j < laserCloudHeight; j++) {
                                int k = 0;
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                                //K维度上依次前移
                                for (; k < laserCloudDepth - 1; k++) {
                                    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)];
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                                    laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];
                                }
                                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeCornerPointer;
                                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
                                laserCloudCubeSurfPointer;
                                laserCloudCubeCornerPointer->clear();
                                laserCloudCubeSurfPointer->clear();
                            }
                        }

                        centerCubeK--;
                        laserCloudCenDepth--;
                    }//调整完成
                    //isFOV中的cube数量
                    int laserCloudValidNum = 0;
                    //周围cube数量
                    int laserCloudSurroundNum = 0;
                    //在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（前后250米范围内），三个维度总共125个cube
                    //在这125个cube里面进一步筛选在视域范围内的cube
                    for (int i = centerCubeI -2; i <= centerCubeI + 2; i++){
                        for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++){
                            for (int k = centerCubeK -2; k <=centerCubeK + 2; k++){
                                if (i >= 0 && i < laserCloudWidth && 
                                    j >= 0 && j < laserCloudHeight && 
                                    k >= 0 && k < laserCloudDepth){//如果索引合法
                                    //换算成实际比例，在世界坐标系下的坐标
                                    float centerX = 50.0 * (i - laserCloudCenWidth);
                                    float centerY = 50.0 * (j - laserCloudCenHeight);
                                    float centerZ = 50.0 * (k - laserCloudCenDepth);

                                    bool isInLaserFOV = false;//判断是否在lidar视线范围的标志（Field of View）
                                    for (int ii = -1; ii <= 1; ii++){
                                        for (int jj = -1; jj <= 1; jj++){
                                            for (int kk = -1; kk <= 1; kk++){
                                                //上下左右八个顶点坐标
                                                //根据余弦定理，Y轴朝上 水平面正负30度可见
                                                float cornerX = centerX + 25.0 * ii;
                                                float cornerY = centerY + 25.0 * jj;
                                                float cornerZ = centerZ + 25.0 * kk;

                                                //原点到顶点距离的平方和
                                                float squaredSide1 = (transformTobeMapped[3] - cornerX) 
                                                                    * (transformTobeMapped[3] - cornerX) 
                                                                    + (transformTobeMapped[4] - cornerY) 
                                                                    * (transformTobeMapped[4] - cornerY)
                                                                    + (transformTobeMapped[5] - cornerZ) 
                                                                    * (transformTobeMapped[5] - cornerZ);

                                                //pointOnYAxis到顶点距离的平方和
                                                float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) 
                                                                    + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
                                                                    + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);

                                                float check1 = 100.0 + squaredSide1 - squaredSide2
                                                            - 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                                                float check2 = 100.0 + squaredSide1 - squaredSide2
                                                            + 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                                                if (check1 < 0 && check2 > 0) {//if |100 + squaredSide1 - squaredSide2| < 10.0 * sqrt(3.0) * sqrt(squaredSide1)
                                                    isInLaserFOV = true;
                                                }
                                            }
                                        }
                                    }
                                    //记住视域范围内的cube索引，匹配用
                                    if (isInLaserFOV) {
                                        laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 
                                                                            + laserCloudWidth * laserCloudHeight * k;
                                        laserCloudValidNum++;
                                    }
                                    //记住附近所有cube的索引，显示用
                                    laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 
                                                                                + laserCloudWidth * laserCloudHeight * k;
                                    laserCloudSurroundNum++;


                                }
                            }
                        }
                    }

                    laserCloudCornerFromMap->clear();
                    laserCloudSurfFromMap->clear();
                    //构建特征点地图，查找匹配使用
                    for (int i = 0; i < laserCloudValidNum; i++) {
                        *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
                        *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
                    }
                    int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
                    int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
                    
                    /***********************************************************************
                    此处将特征点转移回local坐标系，是为了voxel grid filter的下采样操作不越
                    界？好像不是！后面还会转移回世界坐标系，这里是前面的逆转换，和前面一样
                    应无必要，可直接对laserCloudCornerLast和laserCloudSurfLast进行下采样
                    ***********************************************************************/
                    int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();
                    for (int i = 0; i < laserCloudCornerStackNum2; i++){
                        //将世界坐标系下的map点转到local坐标系
                        pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);

                    }
                    int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();
                    for (int i = 0; i < laserCloudSurfStackNum2; i++){
                        //将世界坐标系下的map点转到local坐标系
                        pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);
                    }

                    //进行下采样
                    laserCloudCornerStack->clear();
                    downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
                    downSizeFilterCorner.filter(*laserCloudCornerStack);
                    int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

                    laserCloudSurfStack->clear();
                    downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);
                    downSizeFilterSurf.filter(*laserCloudSurfStack);
                    int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

                    laserCloudCornerStack2->clear();
                    laserCloudSurfStack2->clear();

                    if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100){
                        kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);//构建kd-tree
                        kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
                        
                        for (int iterCount = 0; iterCount < 10; iterCount++){
                            laserCloudOri->clear();
                            coeffSel->clear();

                            //寻找边缘点匹配
                            for (int i = 0; i < laserCloudCornerStackNum; i++){
                                pointOri = laserCloudCornerStack->points[i];
                                //转换回世界坐标系
                                pointAssociateToMap(&pointOri, &pointSel);
                                kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

                                if (pointSearchSqDis[4 < 1.0]){
                                    //将最近的五个点坐标求平均
                                    float cx = 0;
                                    float cy = 0;
                                    float cz = 0;
                                    for (int j = 0; j < 5; j++){
                                        cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
                                        cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
                                        cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
                                    }
                                    cx /= 5;
                                    cy /= 5;
                                    cz /= 5;
                                    
                                    //求均方差
                                    float a11 = 0;
                                    float a12 = 0;
                                    float a13 = 0;
                                    float a22 = 0;
                                    float a23 = 0;
                                    float a33 = 0;
                                    for (int j = 0; j < 5; j++){
                                        float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
                                        float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
                                        float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;

                                        a11 += ax * ax;
                                        a12 += ax * ay;
                                        a13 += ax * az;
                                        a22 += ay * ay;
                                        a23 += ay * az;
                                        a33 += az * az;
                                    }
                                    a11 /= 5;
                                    a12 /= 5;
                                    a13 /= 5;
                                    a22 /= 5;
                                    a23 /= 5;
                                    a33 /= 5;
                                    //构建协方差矩阵
                                    matA1.at<float>(0, 0) = a11;
                                    matA1.at<float>(0, 1) = a12;
                                    matA1.at<float>(0, 2) = a13;
                                    matA1.at<float>(1, 0) = a12;
                                    matA1.at<float>(1, 1) = a22;
                                    matA1.at<float>(1, 2) = a23;
                                    matA1.at<float>(2, 0) = a13;
                                    matA1.at<float>(2, 1) = a23;
                                    matA1.at<float>(2, 2) = a33;

                                    //特征值分解
                                    cv::eigen(matA1, matD1, matV1);
                                    if (matD1.at<float>(0,0) > 3 * matD1.at<float>(0,1)){

                                        float x0 = pointSel.x;
                                        float y0 = pointSel.y;
                                        float z0 = pointSel.z;
                                        //将直线均匀化
                                        float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                                        float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                                        float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                                        float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                                        float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                                        float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                                        //计算OAxOB向量的模
                                        float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                                                    * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                                    + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                                                    * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                                    + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                                                    * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
                                        //计算AB向量的模
                                        float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
                                        //在三个方向上的分量
                                        float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                                + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

                                        float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                                - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                                        float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                                + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                                        float ld2 = a012 / l12;    

                                        //权重系数计算     
                                        float s = 1 - 0.9 * fabs(ld2);

                                        coeff.x = s * la;
                                        coeff.y = s * lb;
                                        coeff.z = s * lc;
                                        coeff.intensity = s * ld2;
                                        //权重大于0.1才计数
                                        if (s > 0.1) {
                                            //原始点云中的点（接收到的边缘点）
                                            laserCloudOri->push_back(pointOri);
                                            coeffSel->push_back(coeff);
                                        }                   
                                    }
                                }
                            }//寻找边缘点匹配结束

                            //开始寻找平面对应点
                            for (int i = 0; i < laserCloudSurfStackNum; i++){
                                pointOri = laserCloudSurfStack->points[i];
                                pointAssociateToMap(&pointOri, &pointOri);
                                kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

                                if (pointSearchSqDis[5 < 1.0]){

                                    //构建五个最近点的坐标矩阵
                                    for (int j = 0; j < 5; j++){

                                        matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
                                        matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
                                        matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
                                    }
                                    //求解matA0 * matX0 = matB0 解出的X0就是平面的法向量
                                    cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);

                                    float pa = matX0.at<float>(0, 0);
                                    float pb = matX0.at<float>(1, 0);
                                    float pc = matX0.at<float>(2, 0);
                                    float pd = 1;
                                    //计算单位法向量
                                    float ps = sqrt(pa * pa + pb * pb + pc * pc);
                                    pa /= ps;
                                    pb /= ps;
                                    pc /= ps;
                                    pd /= ps;   

                                    bool planeValid = true;
                                    for (int j = 0; j < 5; j++){
                                        //if OX * 法向量 > 0.2 说明平面不好
                                        if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
                                            pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
                                            pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) {
                                            planeValid = false;
                                            break;
                                        }
                                    }
                                    if (planeValid){
                                        float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                                        //计算权重
                                        float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                                                + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
                                        
                                        coeff.x = s * pa;
                                        coeff.y = s * pb;
                                        coeff.z = s * pc;
                                        coeff.intensity = s * pd2;

                                        if (s > 0.1){
                                            laserCloudOri->push_back(pointOri);
                                            coeffSel->push_back(coeff);
                                        }
                                    }                                
                                   
                                }
                            }//平面点查找结束

                            float srx = sin(transformTobeMapped[0]);
                            float crx = cos(transformTobeMapped[0]);
                            float sry = sin(transformTobeMapped[1]);
                            float cry = cos(transformTobeMapped[1]);
                            float srz = sin(transformTobeMapped[2]);
                            float crz = cos(transformTobeMapped[2]);

                            int laserCloudSelNum = laserCloudOri->points.size();
                            if (laserCloudSelNum < 50) {
                                continue;
                            }

                            cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
                            cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
                            cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
                            cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
                            cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
                            cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));  

                            for (int i = 0; i < laserCloudSelNum; i++){
                                pointOri = laserCloudOri->points[i];
                                coeff = coeffSel->points[i];

                                //求导这里不太明白
                                float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                                            + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                                            + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

                                float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
                                            + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                                            + ((-cry*crz - srx*sry*srz)*pointOri.x 
                                            + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

                                float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                                            + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                                            + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;

                                matA.at<float>(i, 0) = arx;
                                matA.at<float>(i, 1) = ary;
                                matA.at<float>(i, 2) = arz;
                                matA.at<float>(i, 3) = coeff.x;
                                matA.at<float>(i, 4) = coeff.y;
                                matA.at<float>(i, 5) = coeff.z;
                                matB.at<float>(i, 0) = -coeff.intensity;
                            }
                            //求解
                            cv::transpose(matA, matAt);
                            matAtA = matAt * matA;
                            matAtB = matAt * matB;
                            cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);                          
                            //退化场景判断与处理
                            if (iterCount == 0) {
                                cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
                                cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
                                cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

                                cv::eigen(matAtA, matE, matV);
                                matV.copyTo(matV2);

                                isDegenerate = false;
                                float eignThre[6] = {100, 100, 100, 100, 100, 100};
                                for (int i = 5; i >= 0; i--) {
                                    if (matE.at<float>(0, i) < eignThre[i]) {
                                        for (int j = 0; j < 6; j++) {
                                            matV2.at<float>(i, j) = 0;
                                        }
                                        isDegenerate = true;
                                    } else {
                                        break;
                                    }
                                }
                                matP = matV.inv() * matV2;
                            }

                            if (isDegenerate) {
                                cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
                                matX.copyTo(matX2);
                                matX = matP * matX2;
                            }
                            //积累每次的调整量
                            transformTobeMapped[0] += matX.at<float>(0, 0);
                            transformTobeMapped[1] += matX.at<float>(1, 0);
                            transformTobeMapped[2] += matX.at<float>(2, 0);
                            transformTobeMapped[3] += matX.at<float>(3, 0);
                            transformTobeMapped[4] += matX.at<float>(4, 0);
                            transformTobeMapped[5] += matX.at<float>(5, 0);

                            float deltaR = sqrt(
                                                pow(rad2deg(matX.at<float>(0, 0)), 2) +
                                                pow(rad2deg(matX.at<float>(1, 0)), 2) +
                                                pow(rad2deg(matX.at<float>(2, 0)), 2));
                            float deltaT = sqrt(
                                                pow(matX.at<float>(3, 0) * 100, 2) +
                                                pow(matX.at<float>(4, 0) * 100, 2) +
                                                pow(matX.at<float>(5, 0) * 100, 2));

                            //旋转平移量足够小就停止迭代
                            if (deltaR < 0.05 && deltaT < 0.05) {
                                break;
                            }
                            //迭代结束更新相关的转移矩阵
                            transformUpdate();
                            
                        }
                        





                    

                    }
                    //将corner points归入相应的立方体
                    for (int i = 0; i < laserCloudCornerStackNum; i++){
                        //转移到世界坐标系
                        pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

                        //计算索引
                        int cubeI = int((pointSel.x + 25) / 50) +laserCloudCenWidth;
                        int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
                        int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

                        if (pointSel.x + 25.0 < 0) cubeI--;
                        if (pointSel.y + 25.0 < 0) cubeJ--;
                        if (pointSel.z + 25.0 < 0) cubeK--;

                        if (cubeI >= 0 && cubeI < laserCloudWidth &&
                            cubeJ >= 0 && cubeJ < laserCloudHeight && 
                            cubeK >= 0 && cubeK < laserCloudDepth){
                            
                            //放进不同的组里
                            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; 
                            laserCloudCornerArray[cubeInd]->push_back(pointSel);   
                            }                         
                    }
                    //将surf点放入不同的cube里
                    for (int i = 0; i < laserCloudSurfStackNum; i++){
                        pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);

                        int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
                        int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
                        int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

                        if (pointSel.x + 25.0 < 0) cubeI--;
                        if (pointSel.y + 25.0 < 0) cubeJ--;
                        if (pointSel.z + 25.0 < 0) cubeK--;

                        if (cubeI >= 0 && cubeI < laserCloudWidth && 
                            cubeJ >= 0 && cubeJ < laserCloudHeight && 
                            cubeK >= 0 && cubeK < laserCloudDepth) {
                            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
                            laserCloudSurfArray[cubeInd]->push_back(pointSel);
                        }
                    }

                    //特征点下采样
                    for (int i = 0; i < laserCloudValidNum; i++){
                        int ind = laserCloudValidInd[i];

                        laserCloudCornerArray2[ind]->clear();
                        laserCloudCornerArray2[ind]->clear();
                        downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
                        downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);//滤波输出到Array2

                        laserCloudSurfArray2[ind]->clear();
                        downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
                        downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);

                        //Array与Array2交换，即滤波后自我更新
                        pcl::PointCloud<PointType>::Ptr laserCloudTemp = laserCloudCornerArray[ind];
                        laserCloudCornerArray[ind] = laserCloudCornerArray2[ind];
                        laserCloudCornerArray2[ind] = laserCloudTemp;

                        laserCloudTemp = laserCloudSurfArray[ind];
                        laserCloudSurfArray[ind] = laserCloudSurfArray2[ind];
                        laserCloudSurfArray2[ind] = laserCloudTemp;

                    }

                    mapFrameCount++;
                    //isFOV特征点汇总下采样，每隔五帧publish一次 从第一次开始
                    if (mapFrameCount >= mapFrameNum){
                        mapFrameCount = 0;
                        laserCloudSurround2->clear();
                        for (int i = 0; i < laserCloudSurroundNum; i++){
                            int ind = laserCloudSurroundInd[i];
                            *laserCloudSurround2 += *laserCloudCornerArray[ind];
                            *laserCloudSurround2 += *laserCloudSurfArray[ind];
                        }
                        
                        laserCloudSurround->clear();
                        downSizeFilterCorner.setInputCloud(laserCloudSurround2);
                        downSizeFilterCorner.filter(*laserCloudSurround);

                        sensor_msgs::PointCloud2 laserCloudSurround3;
                        pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
                        laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                        laserCloudSurround3.header.frame_id = "/camera_init";
                        pubLaserCloudSurround.publish(laserCloudSurround3);
                    }
                    //将点云中全部点转移到世界坐标系
                    int laserCloudFullResNum = laserCloudFullRes->points.size();
                    for (int i = 0; i < laserCloudFullResNum; i++){
                        pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);

                    }
                    //发送接收到的点云全部点
                    sensor_msgs::PointCloud2 laserCloudFullRes3;
                    pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
                    laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                    laserCloudFullRes3.header.frame_id = "/camera_init";
                    pubLaserCloudFullRes.publish(laserCloudFullRes3);

                    //发布里程计信息AftMapped
                    //欧拉角转四元数
                    //1.欧拉角转轴角
                    Eigen::AngleAxisd  rx(transformAftMapped[0], Eigen::Vector3d(1,0,0));
                    Eigen::AngleAxisd  ry(transformAftMapped[1], Eigen::Vector3d(0,1,0));
                    Eigen::AngleAxisd  rz(transformAftMapped[2], Eigen::Vector3d(0,0,1));
                    //2.欧拉角转旋转矩阵 旋转顺序为ZXY 
                    Eigen::Matrix3d rotation_matrix = ry.toRotationMatrix() * rx.toRotationMatrix() * rz.toRotationMatrix();
                    //3.旋转矩阵转欧拉角
                    Eigen::Quaterniond q(rotation_matrix);
                    
                    //转为里程计信息
                    odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                    odomAftMapped.pose.pose.orientation.x = q.coeffs().x();
                    odomAftMapped.pose.pose.orientation.y = q.coeffs().y();
                    odomAftMapped.pose.pose.orientation.z = q.coeffs().z();
                    odomAftMapped.pose.pose.orientation.w = q.coeffs().w();
                    odomAftMapped.pose.pose.position.x = transformAftMapped[3];
                    odomAftMapped.pose.pose.position.y = transformAftMapped[4];
                    odomAftMapped.pose.pose.position.z = transformAftMapped[5];
                    //BefMapped
                    odomAftMapped.twist.twist.angular.x = transformBefMapped[0];
                    odomAftMapped.twist.twist.angular.y = transformBefMapped[1];
                    odomAftMapped.twist.twist.angular.z = transformBefMapped[2];
                    odomAftMapped.twist.twist.linear.x = transformBefMapped[3];
                    odomAftMapped.twist.twist.linear.y = transformBefMapped[4];
                    odomAftMapped.twist.twist.linear.z = transformBefMapped[5];
                    pubOdomAftMapped.publish(odomAftMapped);

                    //广播坐标系旋转平移量
                    aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);
                    aftMappedTrans.setRotation(tf::Quaternion(q.coeffs().x(), q.coeffs().y(), q.coeffs().z(), q.coeffs().w()));
                    aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], 
                                             transformAftMapped[4], transformAftMapped[5]));
                    tfBroadcaster.sendTransform(aftMappedTrans);

                }

        }//如果时间相同

        status = ros::ok();
        rate.sleep();
    }//while (status)

    return 0;    
}